//
//  KIT_noWheel.h
//  symbrion
//
//  Created by Berend Weel on 4/3/13.
//  Copyright (c) 2013 Berend Weel. All rights reserved.
//

#ifndef symbrion_KIT_noWheel_h
#define symbrion_KIT_noWheel_h

#include "SymbrionRobot.h"

class KIT_noWheel : public SymbrionRobot {
public:
    virtual void initialiseHardware() {
        SymbrionRobot::initialiseHardware();
        
        initialiseServos();
        initialiseSensors();
        initialiseSensors();
        initialiseLEDs();
        initialiseEmitterReceivers();
        initialiseConnectors();
    }
    
    virtual Servo* getMainHinge(){
        return hinge;
    }
    
private:
    void initialiseServos(){
        hinge = getServo("hinge");
        hinge->enablePosition(TIMESTEP);
    }
    
    void initialiseSensors(){
        distanceSensor.resize(8,NULL);
        for (int i = 0; i < 8; i++) {
			std::string name = "sensor";
			name += boost::lexical_cast<std::string>(i);
			distanceSensor[i] = getDistanceSensor(name);
            distanceSensor[i]->enable(TIMESTEP); // measure once every time step
		}
    }
    
    void initialiseLEDs(){
        led.resize(16,NULL);
        for (int i = 0; i < 16; i++) {
            std::string name = "led";
            name += boost::lexical_cast<std::string>(i);
            
//            std::cout << "Adding led: " << name << std::endl;
            led[i] = Robot::getLED(name);
        }
    }
    
    void initialiseEmitterReceivers(){
        receiver.resize(16,NULL);
        emitter.resize(16,NULL);
        for (int i = 0; i < 16; i++) {
			std::string name = "receiver";
			name += boost::lexical_cast<std::string>(i);
			receiver[i] = getReceiver(name);
            receiver[i]->enable(TIMESTEP); // measure once every time step
		}
        
		for (int i = 0; i < 16; i++) {
			std::string name = "emitter";
			name += boost::lexical_cast<std::string>(i);
			emitter[i] = getEmitter(name);
		}
    }
    
    void initialiseConnectors(){
        connector.resize(4,NULL);
        for (int i = 0; i < 4; i++) {
			std::string name = "connector";
			name += boost::lexical_cast<std::string>(i);
			connector[i] = Robot::getConnector(name);
            connector[i]->enablePresence(TIMESTEP); // measure once every time step
		}
    }
    
    virtual void setLeftSpeed(double speed){
        
    }
    
    virtual void setRightSpeed(double speed){
        
    }
    
    Servo *hinge;
};

#endif
